/*
 * Chris Bena
 * 4/1/11
 *
 * Last Update:
 *  4/12/11
 * 
 * Converted old wall follow code to the new
 *  version of player with hokuyo_aist instead
 *  of laser proxy.
 *
 * Runs the wall hug program using the custom 
 *   odometry to determine the position of the
 *   robot
 *
 * OUTPUT:
 *  on-board odometry: 7_1_OutputData.txt
 *  custom odometry: 7_1_OutputData2.txt
 *
 * KNOWN ISSUES:
 *  occasional script version error - run again
 *  
 */
#include <libplayerc++/playerc++.h>
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <math.h>

#include "args.h"
#include <hokuyo_aist/hokuyo_aist.h>
#include <hokuyo_aist/hokuyo_errors.h>
#include <flexiport/flexiport.h>

#define FRONT 340
#define RIGHT 85
#define LEFT 595
#define FRONT_RIGHT 212
#define FRONT_LEFT 465
#define MIN_RANGE 500 // min range on the right
#define MAX_RANGE 1000 // max range on the right
#define MAX_RANGE_LIMIT 1500 // max range used to find full right hand turns
#define COLLISION 450 // the min range for collision detection
#define LOWERLIMIT 80 // lower limit to ignore garbage laser readings
#define CONS3 0.01 // constant for test 3
#define PI 3.14159265

// Drive State Constants
#define STRAIGHT 0
#define LEFT_TURN 1
#define RIGHT_TURN 2
#define POS_CORRECTION 3
#define TURN_COUNT 35

// max range set to 1.5m for stage; on robot it 
// should be about 0.8-1.0m

using namespace PlayerCc;
    
PlayerClient robot(gHostname, gPort);
Position2dProxy pp(&robot, gIndex);
//LaserProxy lp(&robot, gIndex);
hokuyo_aist::ScanData data;
hokuyo_aist::Sensor laser;
std::ofstream fout;
std::ofstream fout2;
int driveState;    // 0: Straight  1: Left Corner  2: Right Corner

std::string port_options("type=serial,device=/dev/ttyACM0,timeout=1");
double start_angle(0.0), end_angle(0.0);
int first_step(-1), last_step(-1);
int multiecho_mode(0);
unsigned int baud(19200), speed(0), cluster_count(1);
bool get_intensities(false), get_new(false), verbose(false);

/** Debug Vars **/
int debug_driveState=0; // set to one to see the drive state
int stateCountDebug=1;  // set to one to see the drive state count
int speedDebug=0;       // set to one to see the commands sent to the robot
int outputDebug=0;      // set to one to see the lines output to the file
int loopOutDebug=0;     // set to one to see what if statement is hit

/** Turning Vars **/
int rightCount=0;
int leftCount=0;

class Position {
private:
  double x;  // X position
  double y;  // Y position
  double yaw;  // Yaw
  double upperYaw;  // Upper limit of the yaw
  double lowerYaw;  // Lower limit of the yaw
  double distance;  // Total distance gone since the last point
  double lastKnownX;   // The last known X
  double lastKnownY;   // The last known Y
public:	
  Position(){
    x=0;
    y=0;
    yaw=0;
    upperYaw=-275;
    lowerYaw=-85;
    distance=0;
    lastKnownX=0;
    lastKnownY=0;
  }

  void update(double yw, double x0, double y0){  
    // update the new position (x,y)
    int debug=0;
    double newYaw;
    newYaw=rtod(yw);
    if(newYaw>0)  // Fix the on-board yaw to always be between 0 and -360 
      newYaw=newYaw-360;
    if ((newYaw>upperYaw) && (newYaw-upperYaw)<20){ 
      /* Check if the upperYaw has been exceeded and is withing 20 degrees
       *  If yes, increment the yaw by 90 and adjust the new upper and 
       *    lower limits of the next yaw points */
      if (debug)
	std::cout << "yw: " << newYaw << " < lowerYaw: " << lowerYaw 
		  << "   & " << (lowerYaw-newYaw) << std::endl;
      yaw=yaw+90;
      if(yaw > 0)
	yaw=yaw-360;
	upperYaw=yaw+80;
      if(upperYaw>0)
	upperYaw=upperYaw-360;
      lowerYaw=yaw-80;
      if(upperYaw<=-360)
	lowerYaw=lowerYaw+360;
      if (debug)
	std::cout << "yw: " << rtod(yw) << " > upperYaw: " << upperYaw 
		  << "   & " << (rtod(yw)-upperYaw) <<  std::endl;
      //reset();
    }
    else if ((newYaw<lowerYaw) && ((lowerYaw-newYaw)<20)){
      /* Check if the lowerYaw has been passed and is within 20 degrees
       * If yes, decrement the yaw 90 and adjust the new upper and lower
       *   limit of the next yaw points */
            if (debug)
	std::cout << "yw: " << newYaw << " < lowerYaw: " << lowerYaw 
		  << "   & " << (lowerYaw-newYaw) << std::endl;
      yaw=yaw-90;
      if (yaw <= -360)
	yaw=yaw+360;
      // Fix the upper and lower yaw, adjusting for degree wrap around
 
      upperYaw=yaw+80;
      if(upperYaw>0)
	upperYaw=upperYaw-360;
      lowerYaw=yaw-80;
      if(upperYaw<=-360)
	lowerYaw=lowerYaw+360;
	
      if (debug)
	std::cout << "yw: " << rtod(yw) << " < lowerYaw: " << lowerYaw 
		  << "   & " << (lowerYaw-rtod(yw)) << std::endl;
      //reset();
    }
    distance=sqrt((x0-lastKnownX)*(x0-lastKnownX)+(y0-lastKnownY)*(y0-lastKnownY)); // Calc the total distance
    y=y+distance*sin(newYaw); // Update the Y position
    x=x+distance*cos(newYaw); // Update the X position
    lastKnownX=x0; // Set the last known position as the current position
    lastKnownY=y0;
        if (debug==1)
	  std::cout << "yw= " << newYaw << "  yaw= " << yaw 
		<< " lower: " << lowerYaw 
		<< " upper: " << upperYaw
		<< " x: " << x
		<< " y: " << y
		<< std::endl;
  }
  void reset(){  // <- not currently used 
    // Set the odometry to expected values after a change in direction
    //pp.SetOdometry(lastKnownX,lastKnownY,dtor(yaw));
     std::cout<<"set yaw to: " << yaw << "\n";
  }
  double getYaw(){
    // Get the private value of yaw
    return yaw;
  }
  double getX(){
    // Get the private value of X
    return x;
  }
  double getY(){
    // Get the private value of Y
    return y;
  }
};

void turn(double newspeed, double newturnrate){
  pp.SetSpeed(newspeed, newturnrate);
  if(speedDebug){
    std::cout<<"speed:" << newspeed << "  turn:" << newturnrate <<  std::endl;
  }
}

// Turns left - turns until lp[FRONT] is maxed out;
void turn(Position pos1){
  double yaw;
  double x;
  double y;
  do{
    pp.SetSpeed(0.0, 0.375);
    if(speedDebug){
      std::cout<<"speed: 0.0   turn:0.375" << std::endl;
    }
    laser.get_ranges(data,-1,-1,cluster_count);
    robot.Read();
    std::cout << "Still turning... " << data[FRONT] << "\n"; 
    yaw=pp.GetYaw();
    x=pp.GetXPos();
    y=pp.GetYPos();
    pos1.update(yaw,x,y);
  }
  while(data[FRONT] < 1000 && data[FRONT]>=LOWERLIMIT);  // was -- data[FRONT] < 5600
}

void saveData(){
  double x = pp.GetXPos();
  double y = pp.GetYPos();
  double yaw = pp.GetYaw();
  if(outputDebug)
    std::cout << "output " << "x: " << x 
	      <<" y: " <<y 
	      <<" yaw: " <<rtod(yaw)
	      <<" right: " << data[RIGHT] 
	      <<" left: " << data[LEFT] << "\n";
  if(rtod(yaw)>0)
    yaw=dtor(rtod(yaw)-360);
  fout << x; 
  fout << " ";
  fout << y;
  fout << " ";
  fout << rtod(yaw);
  fout << " ";
  fout << data[RIGHT];
  fout << " ";
  fout << data[LEFT] << "\n";
}

void saveData2(double x, double y, double yaw){	
  if(outputDebug)
    std::cout << "output " << "x: " << x 
	      <<" y: " <<y 
	      <<" yaw: " <<yaw
	      <<" right: " << data[RIGHT] 
	      <<" left: " << data[LEFT] << "\n";
  fout2 << x; 
  fout2 << " ";;
  fout2 << y;
  fout2 << " ";
  fout2 << yaw;
  fout2 << " ";
  fout2 << data[RIGHT];
  fout2 << " ";
  fout2 << data[LEFT] << "\n";
}

;
int main(int argc, char **argv)
{
  parse_args(argc,argv);
  
  // we throw exceptions on creation if we fail
  try
    {
      pp.SetMotorEnable (true);
      
      double newspeed = 0;
      double newturnrate = 0;
      
#if defined(WIN32)
      port_options = "type=serial,device=COM3,timeout=1";
#else
      int opt;
      // Get some options from the command line
      while((opt = getopt(argc, argv, "b:c:e:f:il:m:no:s:u:vh")) != -1)
	{
	  switch(opt)
	    {
	    case 'b':
	      sscanf(optarg, "%d", &baud);
	      break;
	    case 'c':
	      sscanf(optarg, "%d", &cluster_count);
	      break;
	    case 'e':
	      sscanf(optarg, "%lf", &end_angle);
	      break;
	    case 'f':
	      sscanf(optarg, "%d", &first_step);
	      break;
	    case 'i':
	      get_intensities = true;
	      break;
	    case 'l':
	      sscanf(optarg, "%d", &last_step);
	      break;
	    case 'm':
	      sscanf(optarg, "%d", &speed);
	      break;
	    case 'n':
	      get_new = true;
	      break;
	    case 'o':
	      port_options = optarg;
	      break;
	    case 's':
	      sscanf(optarg, "%lf", &start_angle);
	      break;
	    case 'u':
	      sscanf(optarg, "%d", &multiecho_mode);
	      break;
	    case 'v':
	      verbose = true;
	      break;
	    case '?':
	    case 'h':
	    default:
	      std::cout << "Usage: " << argv[0] << " [options]\n\n";
	    std::cout << "-b baud\t\tBaud rate to set the laser to "
	      "*after* connecting.\n";
	    std::cout << "-c count\tCluster count.\n";
	    std::cout << "-e angle\tEnd angle to get ranges to.\n";
	    std::cout << "-f step\t\tFirst step to get ranges from.\n";
	    std::cout << "-i\t\tGet intensity data along with ranges.\n";
	    std::cout << "-l step\t\tLast step to get ranges to.\n";
	    std::cout << "-m speed\tMotor speed.\n";
	    std::cout <<
	      "-n\t\tGet new ranges instead of latest ranges.\n";
	    std::cout <<
	      "-o options\tPort options (see flexiport library).\n";
	    std::cout << "-s angle\tStart angle to get ranges from.\n";
	    std::cout << "-u mode\tMulti-echo detection:\n";
	    std::cout << "\t\t0: Off (default), 1: Front, 2: Middle, "
	      "3: Rear, 4: Average\n";
	    std::cout <<
	      "-v\t\tPut the hokuyo_aist library into verbose mode.\n";
	    return 1;
	    }
	}
#endif // defined(WIN32)
      
      //hokuyo_aist::Sensor laser;
      if (verbose)
	laser.set_verbose(true);  // more information in the console
      laser.open(port_options); // Open the later
      
      // Calibration
      std::cout << "Calibrating laser time\n";
      laser.calibrate_time();
      std::cout << "Calculated offset: " << laser.time_offset() << "ns\n";
      std::cout << "Calculated drift rate: " << laser.drift_rate() << '\n';
      std::cout << "Calculated skew alpha: " << laser.skew_alpha() << '\n';
      
      // Turn on the laser
      laser.set_power(true);
      try
	{
	  laser.set_baud(baud);
	}
      catch(hokuyo_aist::BaudrateError &e)
	{
	  std::cerr << "Failed to change baud rate: " << e.what() << '\n';
	}
      catch(hokuyo_aist::ResponseError &e)
	{
	  std::cerr << "Failed to change baud rate: " << e.what() << '\n';
	}
      catch(...)
	{
	  std::cerr << "Failed to change baud rate\n";
	}
      // Set the motor speed
      try
	{
	  laser.set_motor_speed(speed);
	}
      catch(hokuyo_aist::MotorSpeedError &e)
	{
	  std::cerr << "Failed to set motor speed: " << e.what() << '\n';
	}
      catch(hokuyo_aist::ResponseError &e)
	{
	  std::cerr << "Failed to set motor speed: " << e.what() << '\n';
	}
      switch(multiecho_mode)
	{
	case 1:
	  laser.set_multiecho_mode(hokuyo_aist::ME_FRONT);
	  break;
	case 2:
	  laser.set_multiecho_mode(hokuyo_aist::ME_MIDDLE);
	  break;
	case 3:
	  laser.set_multiecho_mode(hokuyo_aist::ME_REAR);
	  break;
	case 4:
	  laser.set_multiecho_mode(hokuyo_aist::ME_AVERAGE);
	  break;
	case 0:
	default:
	  laser.set_multiecho_mode(hokuyo_aist::ME_OFF);
	  break;
	}

      // Get some laser info
      std::cout << "Laser sensor information:\n";
      hokuyo_aist::SensorInfo info;
      laser.get_sensor_info(info);
      std::cout << info.as_string();

      /*  Laser Setup Complete */


      /* Open file out */

      fout.open("7_1_OutputData.txt");
      if(!fout.is_open()){
	std::cerr << "Error opening file\n";
	exit(-1);
      }

      fout2.open("7_1_OutputData2.txt");
      if(!fout2.is_open()){
	std::cerr << "Error opening file\n";
	exit(-1);
      }

      fout.setf(std::ios::fixed, std::ios::floatfield);
      fout.precision(8);

      fout2.setf(std::ios::fixed, std::ios::floatfield);
      fout2.precision(8);

      /* File open complete */
      driveState=STRAIGHT;
      Position pos1;
      pp.ResetOdometry();
      int stateCount=0;

      for(;;)
	{
	
	  // this blocks until new data comes; 10Hz by default
	  laser.get_ranges(data,-1,-1,cluster_count);
	  robot.Read(); 
	  // Using only the certain sensors - front, left, right
	  double front = data[FRONT];
	  double frontRight = data[FRONT_RIGHT];
	  double frontLeft = data[FRONT_LEFT];
	  double right = data[RIGHT];
	  double left = data[LEFT];
	
	  double x=0;
	  double y=0;
	  double yaw = pp.GetYaw();
	  double minRight = 0;
	  double minDistRight = 5600;
	  double minDistLeft = 5600;
	  //driveState=STRAIGHT;
	
	  for(int i = RIGHT; i <= FRONT; i++){
	    if(data[i] < minDistRight && data[i]>=LOWERLIMIT){
	      minDistRight=data[i];
	      minRight=i;
	    }
	  }
	  for(int i = FRONT; i <= LEFT; i++){
	    if(data[i] < minDistLeft && data[i]>=LOWERLIMIT)
	      minDistLeft=data[i];
	  }
	
	  // if inside range
	  if((frontRight <= MIN_RANGE && frontRight>LOWERLIMIT ) ||  minDistRight <= COLLISION){
	    if(loopOutDebug)
	      std::cout << "inside of range  " << frontRight << " " << minDistRight << "\n";
	    newspeed = 0.0;
	    newturnrate = 0.175;
	    turn(newspeed, newturnrate);
	    if(stateCountDebug){
	      if(driveState==POS_CORRECTION)
		stateCount++;
	      else{
		std::cout << "state= " << driveState 
			  << " stateCount= " << stateCount
			  << " to POS_CORRECTION" << std::endl;
		stateCount=0;
	      }
	    }
	    if(rightCount<=2)
	      rightCount=0;
	    if(leftCount<=2)
	      leftCount=0;
	    driveState=POS_CORRECTION;
	  }
	  
	  // else if right turn
	  else if(frontRight > MAX_RANGE_LIMIT || frontRight <= LOWERLIMIT){
	    if(loopOutDebug)
	      std::cout << "Right Turn " << frontRight << " " << "\n";
	    newspeed=0.25;
	    newturnrate=-0.375;
	    turn(newspeed, newturnrate);
	    if(stateCountDebug){
	      if(driveState==RIGHT_TURN)
		stateCount++;
	      else{
		std::cout << "state= " << driveState
			  << " stateCount: " << stateCount 
			  << " to RIGHT_TURN" << std::endl;
		stateCount=0;
	      }
	    }
	    rightCount++;
	    if(rightCount>TURN_COUNT){
	      std::cout<<"Right Turn in right turn check"<<std::endl;
	      rightCount=0;
	    }
	    driveState=RIGHT_TURN;
	  } 
	  
	  // else if outside range
	  else if((frontRight > MAX_RANGE && frontRight < MAX_RANGE_LIMIT) || minDistLeft <= COLLISION){
	    if(loopOutDebug)
	      std::cout << "Outside range" << frontRight << " " << minDistLeft << "\n";
	    newspeed = 0.25;
	    newturnrate = -0.190;
	    turn(newspeed, newturnrate);
	    if(stateCountDebug){
	      if(driveState==POS_CORRECTION)
		stateCount++;
	      else{
		std::cout << "state= " << driveState
			  << " stateCount: " << stateCount 
			  << " to POS_CORRECTION" << std::endl;
		stateCount=0;
	      }
	      if(rightCount<=2)
		rightCount=0;
	      if(leftCount<=2)
		leftCount=0;
	    }
	    driveState=POS_CORRECTION;
	  }
	  
	  // if reached a corner
	  else if(front <= 1500 && frontRight <= 1000 && front>=LOWERLIMIT && frontRight>=LOWERLIMIT){
	    if(loopOutDebug)
	      std::cout << "Turning a corner " << front << " " << frontRight << "\n";
	    newspeed = 0;
	    newturnrate = 0.375;
	    turn(newspeed, newturnrate);
	    if(stateCountDebug){
	      if(driveState==LEFT_TURN)
		stateCount++;
	      else{
		std::cout << "state= " << driveState
			  << " stateCount: " << stateCount 
			  << " to LEFT_TURN" << std::endl;
		stateCount=0;
	      }
	    }
	    leftCount++;
	    if(leftCount>TURN_COUNT){
	      std::cout<<"Left Turn in left turn check"<<std::endl;
	      leftCount=0;
	    }
	    driveState=LEFT_TURN;
	  }

	  else{
	    if(loopOutDebug)
	      std::cout<<"Straight ahead\n";
	    newspeed = 0.5;
	    newturnrate=0;
	    turn(newspeed, newturnrate);
	    if(stateCountDebug){
	      if(driveState==STRAIGHT)
		stateCount++;
	      else{
		std::cout << "state= " << driveState
			  << " stateCount: " << stateCount 
			  << " to STRAIGHT" << std::endl;
		stateCount=0;
	      }
	    }
	    if(rightCount>0){
	      if(rightCount>TURN_COUNT){
		std::cout << "Right Turn: " << rightCount << std::endl;
	      }
	      rightCount=0;
	    }
	    if(leftCount>0){
	      if(leftCount>TURN_COUNT){
		std::cout << "Left Turn: " << leftCount << std::endl;
	      }
	      leftCount=0;
	    }
	    driveState=STRAIGHT;
	  }
	
	  // when we test for front <= 1.0 it gets very close to the wall 
	  // and then starts to turn.  The reason is that there's a while
	  // between when the condition turns true and the robot responds
	  // to the new turn command.
	  if(debug_driveState){
	    std::cout << "Drive State: ";
	    switch(driveState)
	      {
	      case STRAIGHT:
		std::cout << "STRIAGHT";
		break;
	      case LEFT_TURN:
		std::cout << "LEFT TURN";
		break;
	      case RIGHT_TURN:
		std::cout << "RIGHT TURN";
		break;
	      case POS_CORRECTION:
		std::cout << "POSITION CORRECTION";
		break;
	      default:
		break;
	      }
	    std::cout << "\n";
	  }
	  
	  yaw=pp.GetYaw();  // get the yaw from the builtin odometry
	  x=pp.GetXPos();   // get the x from the buildin odometry
	  y=pp.GetYPos();   // get the y from the buildin odometry
	  pos1.update(yaw,x,y);  // update the custom odometry
	  
	  saveData2(pos1.getX(), pos1.getY(), pos1.getYaw());  // save the custom odometry
	  saveData();  // save the built in odometry
	  /**/
	}
      
    }
  
  catch (PlayerCc::PlayerError e)
    {
      std::cerr << e << std::endl;
      return -1;
    }
}
  
